/*
 * @Author: EltonLi lijiaming@uavi-tech.com
 * @Date: 2023-09-11 17:25:42
 * @LastEditors: LVGRAPE
 * @LastEditTime: 2025-06-04 10:41:52
 * @FilePath: \zino-fc-v4\applications\ZINO_FC_V4.0\pid\pid.h
 * @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
 */
/*
 * Copyright (c) 2006-2021, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2023-09-11     UAVI02       the first version
 */
#ifndef __PID_H__
#define __PID_H__

#ifdef __cplusplus
extern "C"
{
#endif

#include <rtthread.h>
#include <rtdef.h>
#include "zino_config.h"

#define PTERM_SCALE 0.032029f
#define ITERM_SCALE 0.024438f
#define DTERM_SCALE 0.000529f

    typedef enum
    {
        PID_RATE_ROLL = 0,
        PID_RATE_PITCH,
        PID_RATE_YAW,
        PID_ANGLE_ROLL,
        PID_ANGLE_PITCH,
        PID_ANGLE_YAW,
        PID_POSITION_VX,
        PID_POSITION_VY,
        PID_POSITION_VZ,
        PID_POSITION_X,
        PID_POSITION_Y,
        PID_POSITION_Z,
        PID_ITEM_COUNT,
    } pidIndex_e;

    typedef struct
    {
        rt_uint16_t P;
        rt_uint16_t I;
        rt_uint16_t D;
    } pid_param_t;

    typedef struct pid_cfg
    {
        pid_param_t pid[PID_ITEM_COUNT];
        uint16_t saveFlag;
        uint16_t baseThrottle;
        uint8_t rcRollPitchMax;
        uint8_t maxTiltAngle;
        uint8_t maxRasing;
    } pid_cfg_t;

    typedef struct pid_obj
    {
        float expect;
        float error;
        float error_pre;
        float inter;
        float deriv;
        float kp;
        float ki;
        float kd;
        float outP;
        float outI;
        float outD;
        float iLimit;
        float outLimit;
        float dt;
        float out;
        float min_dt;
    } pid_obj_t;
    #define PID_CONFIG_SAVE_FLAG 0x55AA
    // extern pid_cfg_t pidConfig;
    extern pid_obj_t pidData[PID_ITEM_COUNT];
    ZINO_CONFIG_DECLARE(pid_cfg_t, pidConfig);
    // ZINO_CONFIG_DECLARE(pid_cfg_t, pidSave);

    void pidInit(pid_obj_t *pid, pid_param_t *param, float dt);
    float pidUpdate(pid_obj_t *pid, float error);
    void pidReset(pid_obj_t *pid);
    float pidUpdate_yaw(pid_obj_t *pid, float error, float lpfK);
    void pid_Reinit_all(void);
    void pid_param_reload(int pidIndex);
    void pid_param_reset_default(void);
    void pidResetAll(void);
    float pid_calculate(pid_obj_t *pid, float sp, float val, float dt);
    float pid_calculate_kp(pid_obj_t *pid, float sp, float val, float dt, float kp);
#ifdef __cplusplus
}
#endif

#endif /* APPLICATIONS_ZINO_FC_V4_0_PID_PID_H_ */
